
#include "translationRotation.h"

#include <math.h> 

void calculate_translation_vector(gsl_vector * v, struct xyz_short * c);
void calculate_rotation_matrix(gsl_matrix * ret, struct xyz_short * center, struct xyz_short * up, struct xyz_short * third) ;


/**
 *  Given o and a calculate sin and cos with one call to sqrt, and some addition, multiplic
ation and division.
*  We want to calculate this:
*  angle = atan(x/y);
*  c = cos(angle);
*  s = sin(angle);
*
*  But:  s = o/h, c = a/h t = o/a    
*  (Sine, Cosine and Tangent of the angle formed by a and h, for a right triangle with sid
es o, a and hypotenuse h)
*
*  Now, by the pythagorean thm:
*
*  h = sqrt(o*o+a*a);
*
*  So, we've avoided three calls to trig functions at the cost of one sqrt and some divisi
ons.
*
* c = a/h;
* s = o/h;
* 
* This macro depends on three local variables:
*   double h, s, c
* h will contain the hypotenuse of the triangle; c will contain cos, and s will contain si
n
*/

#define fast_sin_cos(o, a) h=sqrt(o*o+a*a);c=a/h;s=o/h

/**
   multiply vect by rotmat.  In this file, this is used to rotate vect with the rotation matrix rotmat.
   
   This macro is only defined within calculate_rotation_matrix, and exists in order to
   preserve my sanity only.

   @param rotMat A gsl_matrix_view

*/

#define rotate_center_bbc(rotMat)                                                       \
  gsl_blas_dgemv(CblasNoTrans, 1.0, &(rotMat.matrix), &(up_dv.vector), 0.0, &(tmpv.vector));     \
  gsl_vector_memcpy(&(up_dv.vector), &(tmpv.vector));                                 \
  gsl_blas_dgemv(CblasNoTrans, 1.0, &(rotMat.matrix), &(third_dv.vector), 0.0, &(tmpv.vector));     \
  gsl_vector_memcpy(&(third_dv.vector), &(tmpv.vector));


void calculate_reference_frame(struct xyz_short * center, struct xyz_short * up, struct xyz_short * third, gsl_vector * translation_vector, gsl_matrix * rotation_matrix) {
  
  calculate_translation_vector(translation_vector, center);

  up->x -= center->x;
  up->y -= center->y;
  up->z -= center->z;

  third->x -= center->x;
  third->y -= center->y;
  third->z -= center->z;

  center->x = 0.0;
  center->y = 0.0;
  center->z = 0.0;

  calculate_rotation_matrix(rotation_matrix, center, up, third);
}

void translate_and_rotate(struct xyz_short * c, gsl_vector * translation_vector, gsl_matrix * rotation_matrix) {

  double vector_in[3];
  double vector_out[3];
  
  gsl_vector_view vin_v = gsl_vector_view_array (vector_in, 3) ;
  gsl_vector_view vout_v = gsl_vector_view_array (vector_out, 3) ;
  
  vector_in[0] = (double)c->x;
  vector_in[1] = (double)c->y;
  vector_in[2] = (double)c->z;
  
  gsl_vector_add(&(vin_v.vector), translation_vector);
  gsl_blas_dgemv(CblasNoTrans, 1.0, rotation_matrix, &(vin_v.vector), 0.0, &(vout_v.vector));

  c->x = (short)vector_out[0];
  c->y = (short)vector_out[1];
  c->z = (short)vector_out[2];


}

void rotate(struct xyz_short * c, gsl_matrix * rotation_matrix) {
  double vector_in[3];
  double vector_out[3];
  
  gsl_vector_view vin_v = gsl_vector_view_array (vector_in, 3) ;
  gsl_vector_view vout_v = gsl_vector_view_array (vector_out, 3) ;
  
  vector_in[0] = (double)c->x;
  vector_in[1] = (double)c->y;
  vector_in[2] = (double)c->z;
  
  gsl_blas_dgemv(CblasNoTrans, 1.0, rotation_matrix, &(vin_v.vector), 0.0, &(vout_v.vector));

  c->x = (short)vector_out[0];
  c->y = (short)vector_out[1];
  c->z = (short)vector_out[2];
}

void calculate_translation_vector(gsl_vector * v, struct xyz_short * c) {
  gsl_vector_set(v, 0, -(double)c->x);
  gsl_vector_set(v, 1, -(double)c->y);
  gsl_vector_set(v, 2, -(double)c->z);
}

void calculate_rotation_matrix(gsl_matrix * ret, struct xyz_short * center, struct xyz_short * up, struct xyz_short * third) {

  double up_d[3];
  double third_d[3];
  
  double xaz[9];
  double zax[9];
  double rbb[9];
  double tmpm_a[9];
  double tmpv_a[3];

  gsl_matrix_view xazm, zaxm, rbbm, tmpm;
  gsl_vector_view up_dv, third_dv, tmpv;

  //  double angle;
  double c;
  double s;
  //  double o, a, 
  double h;

  xazm = gsl_matrix_view_array(xaz, 3, 3);
  zaxm = gsl_matrix_view_array(zax, 3, 3);
  rbbm = gsl_matrix_view_array(rbb, 3, 3);

  tmpm = gsl_matrix_view_array(tmpm_a, 3, 3);
  tmpv = gsl_vector_view_array(tmpv_a, 3);

  up_d[0] = (double)up->x;
  up_d[1] = (double)up->y;
  up_d[2] = (double)up->z;
  
  up_dv = gsl_vector_view_array(up_d, 3);
  
  third_d[0] = (double)third->x;
  third_d[1] = (double)third->y;
  third_d[2] = (double)third->z;
 
  third_dv = gsl_vector_view_array(third_d, 3);

  // Rotate the center amino acid into the x = 0 plane about the z axis.
  // (See derivation in notes..)

  //  angle = atan(x/y);
  //  c = cos(angle);
  //  s = sin(angle);

  //      o = x; a = y;
  // so:  o = up_d[0]; a = up_d[1];

  fast_sin_cos(up_d[0], up_d[1]);
  
  xaz[0] =   c; xaz[1] =  -s; xaz[2] = 0.0;
  xaz[3] =   s; xaz[4] =   c; xaz[5] = 0.0;
  xaz[6] = 0.0; xaz[7] = 0.0; xaz[8] = 1.0;

  // Perform the rotation on the center amino acid and it's backBoneCarbon
  rotate_center_bbc(xazm);

  //rot_into_y_about_x

  // Rotate center amino acid into the z = 0 plane about the x axis.
  //  angle = -atan(z/y); = atan(-z/y)
  //  c = cos(angle);
  //  s = sin(angle);

  fast_sin_cos(-up_d[2], up_d[1]);

  zax[0] = 1.0; zax[1] = 0.0; zax[2] = 0.0;
  zax[3] = 0.0; zax[4] =   c; zax[5] =  -s;
  zax[6] = 0.0; zax[7] =   s; zax[8] =   c;

  rotate_center_bbc(zaxm);

  // The direction vector now lies along the y axis.

  // Rotate the backbone into the z=0 plane about the y axis

  // Need to use the backbone atom, not the bond location here...

  //  angle = atan(z/x);
  //  c = cos(angle);
  //  s = sin(angle);

  fast_sin_cos(third_d[2], third_d[0]);

  rbb[0] =   c; rbb[1] = 0.0; rbb[2] =   s;
  rbb[3] = 0.0; rbb[4] = 1.0; rbb[5] = 0.0;
  rbb[6] =  -s; rbb[7] = 0.0; rbb[8] =   c;
  

  rotate_center_bbc(rbbm);

  // Now that we have the matrices, and have rotated the center and back bone carbon of th  neighorhood,
  // calculate the combined rotation matrix to speed further computations.
  
  // ret = rbb * (zax * xaz)
  
  // tmpm = 1.0 * (zax * xaz) + 0.0 * tmpm
  gsl_blas_dgemm (CblasNoTrans, CblasNoTrans, 1.0, &(zaxm.matrix), &(xazm.matrix), 0.0, &(tmpm.matrix)); 
  
  // ret = 1.0 * (rbb * tmpm) * 0.0 * ret
  gsl_blas_dgemm (CblasNoTrans, CblasNoTrans, 1.0, &(rbbm.matrix), &(tmpm.matrix), 0.0, ret); 



}
